import numpy as np
import math
import heapq  # 导入 heapq 模块
import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation
from mpl_toolkits.mplot3d import Axes3D  # noqa: F401
from collections import defaultdict
import pickle
import time

# ====== 配置参数 ======
TIME_SCALE = 100     # 时间离散比例：真实时间 * TIME_SCALE -> 整数刻度
SLACK = 2            # 顶点/边占用时段的缓冲（秒）
SUBSTEPS = 5         # 动画插值步数


# ====== 区间合并工具函数 ======
def merge_intervals(intervals):
    """合并重叠或相邻的时间区间。"""
    if not intervals:
        return []
    intervals.sort(key=lambda x: x[0])
    merged = [list(intervals[0])]
    for s, e in intervals[1:]:
        if s <= merged[-1][1]:
            merged[-1][1] = max(merged[-1][1], e)
        else:
            merged.append([s, e])
    return [(s, e) for s, e in merged]

def is_segment_safe(p1_idx, p2_idx, planner):
    """
    检查两个栅格点之间的直线段是否安全 (Line-of-Sight Check)。
    通过在高密度采样点上检查ESDF值来实现。
    """
    p1_world = np.array([planner.calc_grid_position(p1_idx[i], getattr(planner, f"min_{'xyz'[i]}")) for i in range(3)])
    p2_world = np.array([planner.calc_grid_position(p2_idx[i], getattr(planner, f"min_{'xyz'[i]}")) for i in range(3)])
    direction = p2_world - p1_world
    dist = np.linalg.norm(direction)
    if dist < 1e-6: return True
    
    direction_norm = direction / dist
    num_steps = int(dist / (planner.resolution * 0.5)) + 1 
    
    for i in range(num_steps + 1):
        current_pos_world = p1_world + direction_norm * (i * dist / num_steps)
        ix = planner.calc_xyz_index(current_pos_world[0], planner.min_x)
        iy = planner.calc_xyz_index(current_pos_world[1], planner.min_y)
        iz = planner.calc_xyz_index(current_pos_world[2], planner.min_z)
        if not planner._inside(ix, iy, iz) or planner.esdf_map[ix][iy][iz] < planner.SAFE_DIS:
            return False
    return True

def simplify_zigzag_path(path, planner):
    """
    通过移除不必要的拐点来简化路径 (例如 A->B->C 变成 A->C)。
    保留后续点的时间戳。
    """  
    i = 5  # 开始检查从第i个点开始
    if len(path) < i+2:
        return path
    
    #保留前i个点
    simplified_path = path[:i]
    while i < len(path) - 2:
        p_a = path[i]
        p_b = path[i+1]
        p_c = path[i+2]
        
        # 提取栅格坐标
        a_idx, b_idx, c_idx = p_a[:3], p_b[:3], p_c[:3]

        # 如果直接连接 A->C 是安全的
        if is_segment_safe(a_idx, c_idx, planner):
            # 把 C 点加进来，但是时间戳用它自己的
            simplified_path.append(p_c)
            # 跳过 B 和 C，下一个 A 从 C 开始
            i += 2
        else:
            # 不能简化，正常添加 B 点
            simplified_path.append(p_b)
            i += 1
            
    # 如果循环结束后还有剩余的点（最后一个点），加上它
    if i < len(path):
        # 确保最后一个点被添加
        last_original_point = path[-1]
        if simplified_path[-1] != last_original_point:
             simplified_path.append(last_original_point)

    return simplified_path

class ReservationTable:
    # --- 这个类没有改动 ---
    def __init__(self):
        # 顶点占用：key = (x,y,z) -> [(start,end), ...]
        self.vertex_res = defaultdict(list)
        # 边占用：key = ((x0,y0,z0),(x1,y1,z1)) -> [(start,end), ...]
        self.edge_res = defaultdict(list)

    def reserve_path(self, path):
        """在预留表中记录路径 (x,y,z,raw_t)。"""
        for (x, y, z, raw_t) in path:
            t = int(round(raw_t * TIME_SCALE))
            pad = SLACK * TIME_SCALE
            self.vertex_res[(x, y, z)].append((t - pad, t + pad))
        
        # 边的占用（正向和反向都记录）
        for ((x0, y0, z0, t0), (x1, y1, z1, t1)) in zip(path, path[1:]):
            start = int(round(t0 * TIME_SCALE)) - SLACK * TIME_SCALE
            end   = int(round(t1 * TIME_SCALE)) + SLACK * TIME_SCALE
            interval = (start, end)
            key_fwd = ((x0, y0, z0), (x1, y1, z1))
            key_rev = ((x1, y1, z1), (x0, y0, z0))
            self.edge_res[key_fwd].append(interval)
            self.edge_res[key_rev].append(interval)

        # 合并区间，减少冗余
        for key in list(self.vertex_res.keys()):
            self.vertex_res[key] = merge_intervals(self.vertex_res[key])
        for key in list(self.edge_res.keys()):
            self.edge_res[key] = merge_intervals(self.edge_res[key])

    @staticmethod
    def _overlap(a_start, a_end, b_start, b_end):
        # 判断两个区间是否重叠
        return not (a_end < b_start or a_start > b_end)

    def vertex_conflict(self, x, y, z, t_tick):
        # 判断顶点在给定时刻是否冲突
        for (s, e) in self.vertex_res.get((x, y, z), []):
            if self._overlap(t_tick, t_tick, s, e):
                return True
        return False

    def _iter_crossing_edge_keys(self, x0, y0, z0, x1, y1, z1):
        """生成可能与候选边相交的其它边键值。"""
        dx, dy, dz = x1 - x0, y1 - y0, z1 - z0
        ax, ay, az = abs(dx), abs(dy), abs(dz)
        s = ax + ay + az
        # 面对角
        if s == 2 and ((ax == 1 and ay == 1 and az == 0) or
                       (ax == 1 and ay == 0 and az == 1) or
                       (ax == 0 and ay == 1 and az == 1)):
            if az == 0:  # XY 平面
                a = (x0 + dx, y0, z0)
                b = (x0, y0 + dy, z0)
                yield (a, b)
                yield (b, a)
            elif ay == 0:  # XZ 平面
                a = (x0 + dx, y0, z0)
                b = (x0, y0, z0 + dz)
                yield (a, b)
                yield (b, a)
            elif ax == 0:  # YZ 平面
                a = (x0, y0 + dy, z0)
                b = (x0, y0, z0 + dz)
                yield (a, b)
                yield (b, a)
        # 体对角
        elif s == 3 and ax == 1 and ay == 1 and az == 1:
            e1 = ((x0 + dx, y0, z0), (x0, y0 + dy, z0 + dz))
            e2 = ((x0, y0 + dy, z0), (x0 + dx, y0, z0 + dz))
            e3 = ((x0, y0, z0 + dz), (x0 + dx, y0 + dy, z0))
            for a, b in (e1, e2, e3):
                yield (a, b)
                yield (b, a)

    def edge_conflict(self, x0, y0, z0, x1, y1, z1, t_enter, t_exit):
        # 边本身或反向边冲突
        for key in [((x0, y0, z0), (x1, y1, z1)), ((x1, y1, z1), (x0, y0, z0))]:
            for (s, e) in self.edge_res.get(key, []):
                if self._overlap(t_enter, t_exit, s, e):
                    return True
        # 对角/交叉边冲突
        for (a, b) in self._iter_crossing_edge_keys(x0, y0, z0, x1, y1, z1):
            for (s, e) in self.edge_res.get((a, b), []):
                if self._overlap(t_enter, t_exit, s, e):
                    return True
        return False


class STNode:
    # --- 这个类没有改动 ---
    __slots__ = ("x", "y", "z", "t", "g", "f", "parent")

    def __init__(self, x, y, z, t, g, f, parent):
        self.x, self.y, self.z = x, y, z
        self.t = t  # 连续时间（秒）
        self.g, self.f = g, f
        self.parent = parent

    def __lt__(self, other):
        return self.f < other.f


class SpaceTimeAStar3D:
    """三维时空 A*，避开静态障碍和动态顶点/边冲突。"""

    def __init__(self,
                 reservation: ReservationTable,
                 speed,
                 map_data,      # 从文件加载的地图数据
                 safe_dis,      # ESDF 安全距离（米）
                 max_t=500,
                 heuristic_weight=1.5): # <--- 【改动点】新增启发式权重参数
        # 从地图数据加载参数
        self.resolution = map_data['resolution']
        self.robot_radius = map_data['robot_radius']
        self.min_x = map_data['min_x']
        self.min_y = map_data['min_y']
        self.min_z = map_data['min_z']
        self.max_x = map_data['max_x']
        self.max_y = map_data['max_y']
        self.max_z = map_data['max_z']
        self.x_width = map_data['x_width']
        self.y_width = map_data['y_width']
        self.z_width = map_data['z_width']
        self.obs_map = map_data['obs_map']
        self.esdf_map = map_data['esdf_map']

        self.SAFE_DIS = safe_dis
        self.RT = reservation
        self.speed = speed
        self.max_t = max_t
        self.w = heuristic_weight  # <--- 【改动点】保存权重

        # [动作集]
        self.motion = []
        for dx in (-1, 0, 1):
            for dy in (-1, 0, 1):
                for dz in (-1, 0, 1):
                    if dx == 0 and dy == 0 and dz == 0:
                        continue
                    step_len = math.sqrt(dx * dx + dy * dy + dz * dz)
                    dist = self.resolution * step_len
                    dt = round(dist / self.speed, 4)
                    self.motion.append((dx, dy, dz, dt))
        wait_dt = round(self.resolution / self.speed, 4)
        self.motion.append((0, 0, 0, wait_dt))

    def calc_grid_position(self, index, min_position):
        return index * self.resolution + min_position
       
    def calc_xyz_index(self, pos, min_pos):
        return int(round((pos - min_pos) / self.resolution))

    def _inside(self, x, y, z):
        return (0 <= x < self.x_width and 0 <= y < self.y_width and 0 <= z < self.z_width)

    def heuristic(self, x, y, z, gx, gy, gz):
        wx = self.calc_grid_position(x, self.min_x)
        wy = self.calc_grid_position(y, self.min_y)
        wz = self.calc_grid_position(z, self.min_z)
        gwx = self.calc_grid_position(gx, self.min_x)
        gwy = self.calc_grid_position(gy, self.min_y)
        gwz = self.calc_grid_position(gz, self.min_z)
        dist = math.sqrt((gwx - wx) **2 + (gwy - wy)** 2 + (gwz - wz) **2)
        return dist / self.speed

    def collision_check(self, node: STNode):
        if not self._inside(node.x, node.y, node.z):
            return True
        esdf = self.esdf_map[node.x][node.y][node.z]
        return esdf < self.SAFE_DIS

    def verify_node(self, node: STNode):
        if not self._inside(node.x, node.y, node.z):
            return False
        if self.collision_check(node):
            return False
        return True

    # ===== 路径规划  =====
    def plan(self, sx, sy, sz, gx, gy, gz):
        # 世界坐标 -> 栅格索引
        sx_i = self.calc_xyz_index(sx, self.min_x)
        sy_i = self.calc_xyz_index(sy, self.min_y)
        sz_i = self.calc_xyz_index(sz, self.min_z)
        gx_i = self.calc_xyz_index(gx, self.min_x)
        gy_i = self.calc_xyz_index(gy, self.min_y)
        gz_i = self.calc_xyz_index(gz, self.min_z)

        # 使用 heapq (最小堆) 作为 open_set
        open_heap = []
        open_set_map = {}  # 用于快速查找节点g值: key=(x,y,z,t), value=g
        closed_set = {}

        h0 = self.heuristic(sx_i, sy_i, sz_i, gx_i, gy_i, gz_i)
        f0 = h0 # g0 is 0
        start = STNode(sx_i, sy_i, sz_i, 0.0, 0.0, f0, None)
        goal = STNode(gx_i, gy_i, gz_i, 0.0, 0.0, 0.0, None)

        if not (self.verify_node(start) and self.verify_node(goal)):
            raise RuntimeError("起点或终点不安全（ESDF 检查失败）")

        start_key = (start.x, start.y, start.z, start.t)
        heapq.heappush(open_heap, (start.f, start_key, start))
        open_set_map[start_key] = start.g

        while open_heap:
            # 【改动点】从堆中弹出f值最小的节点，复杂度 O(log N)
            _, cur_key, current = heapq.heappop(open_heap)

            # 如果节点已在 closed_set 中，说明有更优路径已处理过，跳过
            if cur_key in closed_set:
                continue

            # 到达目标
            if current.x == gx_i and current.y == gy_i and current.z == gz_i:
                path = []
                node = current
                while node:
                    path.append((node.x, node.y, node.z, node.t))
                    node = node.parent
                return list(reversed(path))

            # 移动到 closed_set
            closed_set[cur_key] = current
            del open_set_map[cur_key] # 从开放集映射中移除

            # 扩展动作
            for dx, dy, dz, dt_cost in self.motion:
                nx, ny, nz = current.x + dx, current.y + dy, current.z + dz
                raw_nt = round(current.t + dt_cost, 3)
                key2 = (nx, ny, nz, raw_nt)

                if key2 in closed_set:
                    continue

                # [跳过“非必要等待”逻辑，未改动]
                if dx == 0 and dy == 0 and dz == 0:
                    blocked = True
                    for ddx, ddy, ddz, dd_cost in self.motion:
                        if (ddx, ddy, ddz) == (0, 0, 0): continue
                        tx, ty, tz = current.x + ddx, current.y + ddy, current.z + ddz
                        ttest = int(round((current.t + dd_cost) * TIME_SCALE))
                        if (self._inside(tx, ty, tz)
                            and not self.obs_map[tx][ty][tz]
                            and (tx,ty,tz,round(current.t+dd_cost,3)) not in closed_set
                            and not self.RT.vertex_conflict(tx, ty, tz, ttest)
                            and not self.RT.edge_conflict(current.x, current.y, current.z, tx, ty, tz,
                                                          int(round(current.t * TIME_SCALE)), ttest)):
                            blocked = False
                            break
                    if not blocked:
                        continue

                nt = int(round(raw_nt * TIME_SCALE))
                if nt > self.max_t * TIME_SCALE:
                    continue

                g2 = current.g + dt_cost
                # 【改动点】使用加权启发函数
                h2 = self.heuristic(nx, ny, nz, gx_i, gy_i, gz_i)
                f2 = g2 + self.w * h2
                node2 = STNode(nx, ny, nz, raw_nt, g2, f2, current)
                
                if not self.verify_node(node2): continue
                if self.RT.vertex_conflict(nx, ny, nz, nt): continue
                prev_t_scaled = int(round(current.t * TIME_SCALE))
                if self.RT.edge_conflict(current.x, current.y, current.z, nx, ny, nz, prev_t_scaled, nt): continue
                
                # 【改动点】检查并更新 open_set
                if key2 not in open_set_map or g2 < open_set_map[key2]:
                    open_set_map[key2] = g2
                    heapq.heappush(open_heap, (f2, key2, node2))

        raise RuntimeError("三维时空 A* 未能找到可行路径")


# ===== 多无人机规划 (改动区域) =====
def multi_uav_planning_3d(uav_list, map_data, reservation: ReservationTable, 
                          speed, safe_dis=0.5, max_t=500, heuristic_weight=1.5):
    """
    uav_list: [(sx,sy,sz,gx,gy,gz), ...]（世界坐标，单位米）
    ...[其他参数说明不变]...
    heuristic_weight: A* 搜索的启发式权重
    """
    # 为无人机任务增加原始索引并计算路径直线距离
    indexed_uav_list = []
    for i, uav_task in enumerate(uav_list):
        sx, sy, sz, gx, gy, gz = uav_task
        dist = math.sqrt((gx - sx)**2 + (gy - sy)**2 + (gz - sz)**2)
        indexed_uav_list.append({
            'original_index': i,
            'task': uav_task,
            'dist': dist
        })
    
    # 按距离排序，优先规划路径短的无人机
    indexed_uav_list.sort(key=lambda item: item['dist'])

    # 创建 planner 时传入权重
    st_planner = SpaceTimeAStar3D(reservation, speed, map_data, 
                                safe_dis, max_t, heuristic_weight)
    
    path_results = {} # 使用字典存储结果，key为原始索引


    # 按排序后的顺序进行规划
    for item in indexed_uav_list:
        sx, sy, sz, gx, gy, gz = item['task']
        original_index = item['original_index']  
        path = st_planner.plan(sx, sy, sz, gx, gy, gz)      
        print(f"路径找到 长度={len(path)})")
        iterations =2 # 简化迭代次数
        current_path = path
        for _ in range(iterations):
            simplified_path = simplify_zigzag_path(current_path, st_planner)
            current_path = simplified_path
        # 简化路径
        
        print(f"  -> 优化后长度={len(simplified_path)}")
        reservation.reserve_path(simplified_path)
        path_results[original_index] = simplified_path

    
    # 【改动点】按原始顺序重组路径列表，确保输出顺序与输入一致
    all_paths = [path_results[i] for i in range(len(uav_list))]

    return all_paths, st_planner


# ===== 可视化部分 (无改动) =====
def plot_static_trajectories_3d(all_time_paths, planner: SpaceTimeAStar3D):
    # 此函数绘图依赖于 planner.ax，需确保 planner 初始化时创建了图形对象
    # 如果 SpaceTimeAStar3D 的 __init__ 中绘图部分被注释，这里会出错
    # 为保证可运行，临时创建 fig 和 ax
    fig = plt.figure(figsize=(8, 8))
    ax = fig.add_subplot(111, projection='3d')
    ax.set_box_aspect((planner.x_width, planner.y_width, planner.z_width))
    ax.set_title('三维静态地图与路径')
    ax.set_xlabel('X [m]')
    ax.set_ylabel('Y [m]')
    ax.set_zlabel('Z [m]')

    # 绘制障碍物
    xs_obs, ys_obs, zs_obs = [], [], []
    for ix in range(planner.x_width):
        for iy in range(planner.y_width):
            for iz in range(planner.z_width):
                if planner.obs_map[ix][iy][iz]:
                    xs_obs.append(planner.calc_grid_position(ix, planner.min_x))
                    ys_obs.append(planner.calc_grid_position(iy, planner.min_y))
                    zs_obs.append(planner.calc_grid_position(iz, planner.min_z))
    if xs_obs:
        ax.scatter(xs_obs, ys_obs, zs_obs, c='k', s=4, alpha=0.5)


    n = len(all_time_paths)
    colors = plt.get_cmap('tab10')(range(n))

    for idx, path in enumerate(all_time_paths):
        xs = [planner.calc_grid_position(xi, planner.min_x) for xi, _, _, _ in path]
        ys = [planner.calc_grid_position(yi, planner.min_y) for _, yi, _, _ in path]
        zs = [planner.calc_grid_position(zi, planner.min_z) for _, _, zi, _ in path]
        ax.plot(xs, ys, zs, linewidth=2, color=colors[idx], label=f'UAV {idx+1}')
        ax.scatter(xs[0], ys[0], zs[0], marker='o', s=50, color=colors[idx])
        ax.scatter(xs[-1], ys[-1], zs[-1], marker='^', s=60, color=colors[idx])

    ax.legend(loc='best')

def animate_uav_trajectories_3d(all_time_paths, planner: SpaceTimeAStar3D, interval=120):
    # 1) 为每架无人机构建关键帧（时间 -> 位置）
    keyframes = []
    frame_times = set()
    for path in all_time_paths:
        d = {}
        for xi, yi, zi, t in path:
            f = int(round(t * TIME_SCALE))
            xw = planner.calc_grid_position(xi, planner.min_x)
            yw = planner.calc_grid_position(yi, planner.min_y)
            zw = planner.calc_grid_position(zi, planner.min_z)
            d[f] = (xw, yw, zw)
            frame_times.add(f)
        keyframes.append(d)

    base_frames = sorted(frame_times)
    if len(base_frames) < 2:
        base_frames = base_frames + [base_frames[0] + 1]

    # 2) 构造平滑时间轴（插值）
    smooth_times = []
    if len(base_frames) > 1:
        for i in range(len(base_frames) - 1):
            t0, t1 = base_frames[i], base_frames[i + 1]
            dt = (t1 - t0) / SUBSTEPS if SUBSTEPS > 0 else (t1 - t0)
            for k in range(SUBSTEPS):
                smooth_times.append(t0 + k * dt)
    smooth_times.append(base_frames[-1])

    # 3) 对每架无人机进行线性插值
    uav_smooth = []
    for d in keyframes:
        times = np.array(sorted(d.keys()), dtype=float)
        if not times.any(): continue
        xs = np.array([d[t][0] for t in times])
        ys = np.array([d[t][1] for t in times])
        zs = np.array([d[t][2] for t in times])
        xi = np.interp(smooth_times, times, xs)
        yi = np.interp(smooth_times, times, ys)
        zi = np.interp(smooth_times, times, zs)
        uav_smooth.append((xi, yi, zi))

    # 4) 绘图初始化
    fig = plt.figure(figsize=(8, 8))
    ax = fig.add_subplot(111, projection='3d')
    ax.set_box_aspect((planner.x_width, planner.y_width, planner.z_width))

    xs, ys, zs = [], [], []
    for ix in range(planner.x_width):
        for iy in range(planner.y_width):
            for iz in range(planner.z_width):
                if planner.obs_map[ix][iy][iz]:
                    xs.append(planner.calc_grid_position(ix, planner.min_x))
                    ys.append(planner.calc_grid_position(iy, planner.min_y))
                    zs.append(planner.calc_grid_position(iz, planner.min_z))
    if xs:
        ax.scatter(xs, ys, zs, c='k', s=4, alpha=0.4)

    if not uav_smooth: return None
    all_x = np.concatenate([pos[0] for pos in uav_smooth])
    all_y = np.concatenate([pos[1] for pos in uav_smooth])
    all_z = np.concatenate([pos[2] for pos in uav_smooth])
    ax.set_xlim(all_x.min() - 1, all_x.max() + 1)
    ax.set_ylim(all_y.min() - 1, all_y.max() + 1)
    ax.set_zlim(all_z.min() - 1, all_z.max() + 1)
    ax.set_title('多无人机三维轨迹动画')
    ax.set_xlabel('X [m]')
    ax.set_ylabel('Y [m]')
    ax.set_zlabel('Z [m]')

    n = len(uav_smooth)
    colors = plt.get_cmap('tab10')(range(n))
    scatters = [ax.scatter([], [], [], s=40, color=colors[i]) for i in range(n)]
    trails = [ax.plot([], [], [], linewidth=1, color=colors[i], alpha=0.8)[0] for i in range(n)]

    def update(i):
        for idx, (xi, yi, zi) in enumerate(uav_smooth):
            x, y, z = xi[i], yi[i], zi[i]
            scatters[idx]._offsets3d = ([x], [y], [z])
            trails[idx].set_data(xi[:i + 1], yi[:i + 1])
            trails[idx].set_3d_properties(zi[:i + 1])
        ax.set_xlabel(f'时间: {smooth_times[i] / TIME_SCALE:.2f}s')
        return scatters + trails

    anim = FuncAnimation(fig, update, frames=len(smooth_times), interval=interval, blit=False)
    return anim


def load_map_data(filename):
    """加载保存的地图数据"""
    with open(filename, 'rb') as f:
        map_data = pickle.load(f)
    print(f"已加载地图数据: {filename}")
    print(f"地图尺寸: {map_data['x_width']}x{map_data['y_width']}x{map_data['z_width']}")
    return map_data


if __name__ == "__main__":
    # 加载预先生成的地图
    map_data = load_map_data("3d_environment_fine_map.pkl")
    # 创建预留表（外部维护）
    reservation = ReservationTable()
    
    # 增加无人机数量以测试性能
    uav_list = [
        (0.0, 0.0, 0.0, 30.0, 30.0, 20.0),
        (0.0, 3.0, 0.0, 30.0, 27.0, 22.0),
        (5.0, 0.0, 2.0, 28.0, 30.0, 25.0),
        (30.0, 0.0, 5.0, 0.0, 30.0, 15.0), # 增加反向任务
        (0.0, 30.0, 3.0, 30.0, 0.0, 18.0), # 增加交叉任务
        (2.0, 2.0, 2.0, 28.0, 28.0, 28.0),
        (28.0, 2.0, 4.0, 2.0, 28.0, 26.0),
        (2.0, 28.0, 6.0, 28.0, 2.0, 24.0),
        (28.0, 28.0, 8.0, 2.0, 2.0, 22.0),
        (15.0, 2.0, 10.0, 15.0, 28.0, 10.0),
    ]

    # 进行路径规划
    # 可以调整 heuristic_weight, 推荐值 1.1 ~ 2.0。值越大搜索越快，但路径质量越可能下降
    all_time_paths, st_planner = multi_uav_planning_3d(
        uav_list, map_data, reservation, 
        speed=6.0, safe_dis=0.8, max_t=300, heuristic_weight=1.5
    )

    # 打印路径
    for i, path in enumerate(all_time_paths):
        print(f"UAV{i+1} 路径点数量: {len(path)}")

    # 静态三维图
    plot_static_trajectories_3d(all_time_paths, st_planner)

    # 三维动画
    anim = animate_uav_trajectories_3d(all_time_paths, st_planner, interval=150)
    plt.show()
